% **************************************************************************
% 函 数 名：GTGyro.m
% 函数功能：动力调谐陀螺仪示意图
% 作    者：TMRNic
% 创建时间：2023-11-08
% 其他说明：静止状态
% 参考信息：《惯性导航》（第二版）科学出版社, 2016.
% 修改信息：无
% **************************************************************************
start;
addpath MatGraphLib\;
N = 400;            % 仿真长度
avp=zeros(9,1);     % 初始avp，在此动画中没有用位置速度
sx = 0;  xx = 0;
plan = 4;           % 飞机尺寸，此动画中没有意义

figure;
for i=1:N
    hold off;
    psi = 18*glv.deg*i;         % 俯仰角
    theta = -(pi/12)*cos(pi/5*i)-pi/12;     % 航向角
    
    Cnb = a2mat([-pi/6, 0, psi]);       % 陀螺转子姿态矩阵
    r1 = 10; r2 = 8;  h1 = 2;  h2 = -2;  C = 1;
    GML_Ring_3D(r1,r2,h1,h2, Cnb', C);  % 画陀螺转子
    
    
    Cnb = a2mat([theta, 0, psi]);       % 平衡环姿态矩阵
    r1 = 6; r2 = 4;  C=2;
    GML_Ring_3D(r1,r2,h1,h2, Cnb', C);   % 画平衡环
    
    p0 = [0;8;0];  p1 = [0;6;0];  width = 6;
    GML_Line(p0, p1, Cnb', width);      % 外扭杆
    GML_Line(-p0, -p1, Cnb', width);
    
    Cnb = a2mat([0, 0, psi]);           % 内扭杆姿态阵
    p0 = [4;0;0];  p1 = [2;0;0];  width = 6;
    GML_Line(p0, p1, Cnb', width);      % 内扭杆
    GML_Line(-p0, -p1, Cnb', width);

    r = 2;  h1 = 3;  h2 = -8;
    GML_Cylinder(r, Cnb, h1, h2)        % 电机轴
    axis([xx-3*plan sx+3*plan xx-3*plan sx+3*plan xx-3*plan sx+3*plan]); grid on    % 绘制坐标系统
    drawnow
    if i==1,  pause(3); end
end


